#include "parser.h"

unordered_map<int64_t, std::vector<std::string>> Parser::undecoder_points_sequence = {{0.0, {"0"}}};
std::ofstream Parser::f_out("../timestamp.txt");
Parser::Parser(const char* filepath)
{
    ros::NodeHandle nh;
    point_pub = nh.advertise<sensor_msgs::PointCloud2> ("point_cloud_VLP", 1000);

    // cloud = pcl::PointCloud<pcl::PointXYZI>::Ptr(new pcl::PointCloud<pcl::PointXYZI>);
    char ebuf[PCAP_ERRBUF_SIZE];
    p = pcap_open_offline(filepath, ebuf);
    if (p == nullptr) {
        std::cerr << "Error: Could not open file " << filepath << ": " << ebuf << std::endl;
    }

    // f_out.open("../timestamp.txt");
}

Parser::~Parser()
{
    pcap_close(p);
    f_out.close();
}

void Parser::udpframe_decode(const std::string &packet_header, int &point_count, const int64_t &last_timestamp, pcl::PointCloud<pcl::PointXYZI> &cloud)
{
    const ethhdr *ethernet;                  /* The ethernet header [1] */
    const struct packet_ip *ip;              /* The IP header */
    const udphdr* udp;                       /* The UDP header */
    int size_ip;                             // ip 头的长度

    int64_t curr_timestamp = std::atof(packet_header.substr(1248, packet_header.length() - 1248).c_str());

    std::string packet_str = packet_header.substr(0, 1248);
    u_char packet[packet_str.length() + 1];
    memcpy(packet, packet_str.c_str(), packet_str.length() + 1);
    // /* IP头 */
    ip = (struct packet_ip*)(packet + SIZE_ETHERNET);
    size_ip = IP_HL(ip)*4;

    // 这是12个data block起始的位置
    int data_start = SIZE_ETHERNET + SIZE_UDP + size_ip;
    int offset = data_start;
    
    // 一共有12个block
    for(int block_id = 0; block_id < 12; block_id++)
    {
        
        // 4字节，保存的是 标志位 0xFFEE 以及 角度
        // 注意这里的0xFFEE是低位 FF 高位 EE，因此在比较的时候，要反过来，因为高位在前，低位在后，所以是 0xEEFF
        assert(*(uint16_t*)(packet + offset) == 0xEEFF);     
        const uint16_t* angle = (uint16_t*)(packet + offset + 2);
        float block_alpha = *angle * 0.01;
        if(block_alpha >= 360.0)
            block_alpha -= 360.0; 
        offset += 4;

        if(curr_timestamp < last_timestamp)
        {
            offset += 3 * SCANS;
            
            goto next_block;
        }
        for(int laser_id = 0; laser_id < SCANS; laser_id++)
        {
            float distance = *(uint16_t*)(packet + offset) * 0.002;     // 这里保存的距离是 2mm为单位的
            offset += 3;            // 每个点是3个字节保存的
            if(distance == 0)
                continue;
            u_char reflection = *(packet + offset + 2);
            float point_alpha = block_alpha + laser_id * FIRING_CYCLE / 1000000.0 * HORIZON_SPEED;
            point_alpha *= M_PI / 180.0;
            float x = distance * sin(point_alpha) * cos(vertical_angle[laser_id]);
            float y = distance * cos(point_alpha) * cos(vertical_angle[laser_id]);
            float z = distance * sin(vertical_angle[laser_id]);
            z += vertical_correction[laser_id] * 0.01;

            pcl::PointXYZI point;
            point.x = x;
            point.y = y; 
            point.z = z;
            point.intensity = reflection;
            cloud.push_back(point);
            point_count++;
            
        }
        next_block:
        block_alpha += SEQUENCE_TIME / 1000000.0 * HORIZON_SPEED;
        curr_timestamp += SEQUENCE_TIME;    //modify
    }
}

void Parser::read_callback(u_char *args, const struct pcap_pkthdr *header, const u_char *packet)
{
    const ethhdr *ethernet;  /* The ethernet header [1] */
    const struct packet_ip *ip;              /* The IP header */
    const udphdr* udp;                       /* The UDP header */
    int size_ip;                          // ip 头的长度

    int64_t *last_timestamp = (int64_t*)args;
    // 512 字节的是Position Packet，一般是外接了其他设备时候使用的，比如GPS IMU等
    // 这里只有雷达，所以直接跳过
    if(header->len != 1206 + SIZE_ETHERNET + SIZE_UDP + 20)
    {
        // (*counter)++;
        return ;
    }
    int64_t curr_timestamp = (header->ts.tv_sec + header->ts.tv_usec / 1000000.0) * 1000000; //modify
    // 如果没给定初始的时间戳，那就把第一帧时间戳当做初始的
    if(*last_timestamp <= 0)
        *last_timestamp = curr_timestamp;
    /* 以太网头 */
    ethernet = (ethhdr*)(packet);

    /* IP头 */
    ip = (struct packet_ip*)(packet + SIZE_ETHERNET);
    size_ip = IP_HL(ip)*4;
    if (size_ip < 20) {
        cout << "无效的IP头长度: "<<  size_ip  << " bytes" << endl;
        return;
    }

    // 必须得是UDP协议才正确
    if ( ip->ip_p != IPPROTO_UDP )
    { 
        cout << "protocal is not UDP, wrong. protocal is " << int(ip->ip_p) << endl;
        return ;
    }
    /* UDP 头*/
    udp = (udphdr*)(packet + SIZE_ETHERNET + size_ip);
    int source_port = ntohs(udp->source);
    int target_port = ntohs(udp->dest);
    // cout << "source_port : " << source_port << ", target port: " << target_port << endl;

    // 这是12个data block起始的位置
    int data_start = SIZE_ETHERNET + SIZE_UDP + size_ip;
    const u_char* mode = (u_char*)(packet + data_start + 1204);
    if(*mode == DUAL_RETURN )
    {
        cout << "dual return mode is not support" << endl;
        return;
    }

    /*存入未解密数据*/
    std::stringstream ss;
    ss.precision(10);
    ss << std::fixed << curr_timestamp;
    std::string data_header_str = ss.str();

    u_char data_packet[1206 + SIZE_ETHERNET + SIZE_UDP + 20];
    memcpy(data_packet, packet, 1206 + SIZE_ETHERNET + SIZE_UDP + 20);
    std::string data_packet_str(reinterpret_cast<const char*>(data_packet), 1206 + SIZE_ETHERNET + SIZE_UDP + 20);

    undecoder_points_sequence[*last_timestamp].push_back(data_packet_str + data_header_str);
    /*存入未解密数据*/

    // 一共有12个block
    for(int block_id = 0; block_id < 12; block_id++)
    {       
        if(curr_timestamp < *last_timestamp)
        {
            goto next_block;
        }
        next_block:
        curr_timestamp += SEQUENCE_TIME; //modify
        
        if(curr_timestamp - *last_timestamp >= PCD_DURATION * 1000000) //modify
        {
            f_out << fixed << setprecision(0) << *last_timestamp << std::endl;
            *last_timestamp = curr_timestamp;
            
        }  
    }
}

void Parser::init()
{
    ROS_INFO("************  start init!  ************\n");
    undecoder_points_sequence.clear();
    u_char data[sizeof(int64_t)] = {0};
    int64_t start_timestamp = 0;
    *(int64_t*)data = start_timestamp;
    pcap_loop(p, -1, read_callback, (u_char*)data);
}

void Parser::decoder_points()
{
    ROS_INFO("*******  start decoder_points! ********\n");
    for(const pair<int64_t, std::vector<std::string>>& kv: undecoder_points_sequence)
    {    
        std::vector<std::string> udp_frames = kv.second;
        if (abs(kv.first - 1605416068558455) < 50000)
        {
        std::cout << kv.first <<std::endl;
        // one_points_frame(udp_frames, kv.first);
        one_points_frame(udp_frames, 1605416068596306);
        
        }
            
    }
}

void Parser::one_points_frame(std::vector<std::string> udp_frames, const int64_t &last_timestamp)
{
    int point_count = 0;
    pcl::PointCloud<pcl::PointXYZI> cloud;
    cloud.clear();
    for(int i = 0; i < udp_frames.size(); i++)
    {
        udpframe_decode(udp_frames[i], point_count, last_timestamp, cloud);
    }
    // std::cout  << point_count << std::endl;
    sensor_msgs::PointCloud2 output;
    pcl::toROSMsg(cloud, output);
    output.header.frame_id = "odom";
    ros::Rate loop_rate(10);
    point_pub.publish(output);
    loop_rate.sleep();
}

pcl::PointCloud<pcl::PointXYZI> Parser::get_point_frame(const int64_t &timestamp)
{
    std::vector<std::string> points = undecoder_points_sequence[timestamp];
    int point_count = 0;
    pcl::PointCloud<pcl::PointXYZI> cloud;
    cloud.clear();
    for(int i = 0; i < points.size(); i++)
    {
        udpframe_decode(points[i], point_count, timestamp, cloud);       
    }
    return cloud;
}